#include "./ctrl.h"
#include "../device/gpio.h"
#include "./sensor.h"
#include "./analog.h"
#include "../utils/log.h"

namespace driver {

Ctrl ctrl;

constexpr uint32_t COL_COUNT = 13;

using namespace device;
using namespace utils;

static void delay() {
    for (volatile uint32_t i = 0; i < 100; i ++);
   // osDelay(10);
}

namespace u1d_u2d {
using A0= PB<3>;
using A1 = PB<4>;
using A2 = PB<5>;
using D1 = PB<0>;
using LE1 = PB<1>;
using D2 = PA<7>;
using LE2 = PA<15>;

static void select(uint8_t addr) {
    if (addr & 0x01) {
        A0::set();
    } else {
        A0::clr();
    }
    if (addr & 0x02) {
        A1::set();
    } else {
        A1::clr();
    }
    if (addr & 0x04) {
        A2::set();
    } else {
        A2::clr();
    }
}

static void write_u1d(uint8_t addr, bool val) {
    select(addr);
    if (val) {
        D1::set();
    } else {
        D1::clr();
    }

    LE1::clr();
    delay();
    LE1::set();
    delay();
}

static void write_u2d(uint8_t addr, bool val) {
    select(addr);
    if (val) {
        D2::set();
    } else {
        D2::clr();
    }

    LE2::clr();
    delay();
    LE2::set();
    delay();
}

}


namespace u3d_u4d {
using A0 = PC<10>;
using A1 = PC<11>;
using A2 = PC<12>;
using D3 = PC<0>;
using LE3 = PC<1>;
using D4 = PC<8>;
using LE4 = PC<7>;

static void select(uint8_t addr) {
    if (addr & 0x01) {
        A0::set();
    } else {
        A0::clr();
    }
    if (addr & 0x02) {
        A1::set();
    } else {
        A1::clr();
    }
    if (addr & 0x04) {
        A2::set();
    } else {
        A2::clr();
    }
}

static void write_u3d(uint8_t addr, bool val) {
    select(addr);
    if (val) {
        D3::set();
    } else {
        D3::clr();
    }

    LE3::clr();
    delay();
    LE3::set();
    delay();
}

static void write_u4d(uint8_t addr, bool val) {
    select(addr);
    if (val) {
        D4::set();
    } else {
        D4::clr();
    }

    LE4::clr();
    delay();
    LE4::set();
    delay();
}

}

using Beep = PD<2>;

void Ctrl::init() {

    RCU_APB2EN |= RCU_APB2EN_PAEN |
        RCU_APB2EN_PCEN |
        RCU_APB2EN_PDEN |
        RCU_APB2EN_PBEN;

    config_od_output<Beep>();

    config_pp_output<u1d_u2d::A0, u1d_u2d::A1, u1d_u2d::A2,
                     u1d_u2d::D1, u1d_u2d::D2, u1d_u2d::LE1, u1d_u2d::LE2,
                     u3d_u4d::A0, u3d_u4d::A1, u3d_u4d::A2, 
                     u3d_u4d::D3, u3d_u4d::D4, u3d_u4d::LE3, u3d_u4d::LE4
                >();
    reset();
    beep_off();
}

void Ctrl::beep_on() {
    Beep::clr();
}

void Ctrl::beep_off() {
    Beep::set();
}

static void select_col(uint8_t col, bool val) {
    if (col <= 8) {
        u3d_u4d::write_u3d(col - 1, val);
    } else {
        u3d_u4d::write_u4d(col - 1 - 8, val);
    }
}

static void select_row(uint8_t row, bool val) {
    if (row <= 8) {
        u1d_u2d::write_u1d(row - 1, val);
    } else {
        u1d_u2d::write_u2d(row - 1 - 8, val);
    }
}

void Ctrl::reset() {
    for (uint8_t i = 0; i < 8; i ++) {
        u1d_u2d::write_u1d(i, false);
        u1d_u2d::write_u2d(i, false);
        u3d_u4d::write_u3d(i, false);
        u3d_u4d::write_u4d(i, false);
    }
    for (uint8_t i = 0; i < 8; i ++) {
        select_row(i + 1, false);
    }
    for (uint8_t i = 0; i < COL_COUNT; i ++) {
        select_col(i + 1, true);
    }
}

void Ctrl::close_all() {
    for (uint8_t i = 0; i < 8; i ++) {
        select_row(i + 1, false);
    }
    for (uint8_t i = 0; i < COL_COUNT; i ++) {
        select_col(i + 1, true);
    }
}

void Ctrl::select(uint8_t row, uint8_t col, bool val) {
    select_row(row, val);
    select_col(col, !val);
}

Error Ctrl::run_with_state(uint8_t row, uint8_t col, bool state) {
    Error err;
    uint32_t time_count = 0;
    uint32_t lock_count = 0;
    uint32_t check_count = 0;

    close_all();

    select(row, col, true);

    while (true) {

        osDelay(15);
        time_count += 15;

        if (time_count >= RUN_TIMEOUT) {
            err = ec::MOTOR_TIMEOUT;
            break;
        }

        if (sensor.is_col_checked(col) == state) {
            check_count ++;
            if (check_count >= 3) {
                break;
            }
        } else {
            check_count = 0;
        }

        if ((time_count > 500) && analog.is_motor_locked()) {
            lock_count ++;
            if (lock_count >= 10) {
                err = ec::MOTOR_LOCKED;
                break;
            }
        } else {
            lock_count = 0;
        }
    }
finish:
    select(row, col, false);
    return err;
}

void select_with_mask(uint32_t col_mask) {
    for (uint8_t i = 0; i < 13; i ++) {
        select_col(i + 1, (col_mask & (0x01 << i)) != 0 );
       // osDelay(2);
    }
}

Error Ctrl::run_pick(uint8_t row, uint8_t col) {
    sensor.clear_raster();
    uint32_t time_count = 0;
    uint32_t lock_count = 0;
    Error err;

    if (sensor.is_raster_check()) {
        err = ec::RASTER_INVALID;
        goto finish;
    }

    led_close();
    close_all();

    select(row, col, true);
    for (;;) {
        if (sensor.is_raster_check()) {
            break;
        }

        osDelay(20);
        time_count += 20;

        if (time_count >= RUN_TIMEOUT) {
            err = ec::MOTOR_TIMEOUT;
            goto finish;
        }

        if ((time_count > 500) && analog.is_motor_locked()) {
            lock_count ++;
            if (lock_count >= 10) {
                err = ec::MOTOR_LOCKED;
                goto finish;
            }
        } else {
            lock_count = 0;
        }
    }
    led_open();
    recount_led_close();
finish:
    select(row, col, false);
    return err;
}

Error Ctrl::run_with_state(uint8_t row, bool state) {
    Error err;
    uint32_t time_count = 0;
    uint32_t cols = 0;
    constexpr uint32_t ALL = (0x01 << COL_COUNT) - 1;

    select_row(row, true);
    select_with_mask(0);
    osDelay(2000);
    // while (cols != ALL) {

    //     for (uint8_t i = 0; i < COL_COUNT; i ++) {
    //         uint32_t mask = 0x01 << i;
    //         bool ret = sensor.is_col_checked(i + 1);
    //         if (((cols & mask) == 0) && (ret == state)) {
    //             cols |= mask;
    //             select_col(i + 1, true);
    //             osDelay(2);
    //         }
    //     }

    //     osDelay(50);
        
    //     time_count += 50;
    //     if (time_count >= RUN_TIMEOUT) {
    //         err = ec::MOTOR_TIMEOUT;
    //         break;
    //     }
    // }

finish:
    select_row(row, false);
    select_with_mask(ALL);

    return err;
}

void Ctrl::led_open() {
    select_row(11, true);
}

void Ctrl::led_close() {
    select_row(11, false);
}

}